#include "MovementController.h"
#include "Arduino.h"

MovementController::MovementController(Wheel leftWheels[], int numOfLeftWheels, Wheel rightWheels[], int numOfRightWheels):
{
  _leftSide = new Wheel[numOfLeftWheels];
  _rightSide = new Wheel[numOfRightWheels];
  
  _initialized = false; 
}

MovementController::~MovementController(){
  delete [] _leftSide;
  delete [] _rightSide;
}

MovementController::initialize(){

	//Setup enable all wheel pins on the left side and write a digital HIGH to the enable pins.
	for(int i = 0; i < _numOfLeftWheels; i++){
		pinMode(_leftSide[i].getEnablePin(), OUTPUT);
		digitalWrite(leftWheels[i].getEnablePin(), HIGH); 
		pinMode(leftWheels[i].getFrontPin(), OUTPUT);
		pinMode(leftWheels[i].getBackPin(), OUTPUT);
	}

	//Do ths same for the right side.
	for(int i = 0; i < _numOfRightWheels; i++){
		pinMode(_rightSide[i].getEnablePin(), OUTPUT);
		digitalWrite(rightWheels[i].getEnablePin(), HIGH);
		pinMode(rightWheels[i].getFrontPin(), OUTPUT);
		pinMode(rightWheels[i].getBackPin(), OUTPUT); 
	}

	_initialized = true;

  }
}

MovementController::moveLeftSideForward(int duty){
	if(_initialized && duty <= 255 && duty >= 0){
	  for (int i = 0; i < _numOfLeftWheels; i++){
		analogWrite(_leftWheels[i].getFrontPin(), duty);
		analogWrite(_leftWheels[i].getBackPin(), 0);
	  }
	}
}

MovementController::moveLeftSideBackward(int duty){
	if(_initialized && duty <= 255 && duty >= 0){
	  for (int i = 0; i < _numOfLeftWheels; i++){
		analogWrite(_leftWheels[i].getFrontPin(), 0);
		analogWrite(_leftWheels[i].getBackPin(), duty);
	  }
	}
}

MovementController::moveRightSideForward(int duty){
	if(_initialized && duty <= 255 && duty >= 0){
	  for (int i = 0; i < _numOfRightWheels; i++){
		analogWrite(_rightWheels[i].getFrontPin(), duty);
		analogWrite(_rightWheels[i].getBackPin(), 0);
	  }
	}
}

MovementController::moveRightSideBackward(int duty){
	if(_initialized && duty <= 255 && duty >= 0){
	  for (int i = 0; i < _numOfRightWheels; i++){
		analogWrite(_rightWheels[i].getFrontPin(), 0);
		analogWrite(_rightWheels[i].getBackPin(), duty);
	  }
	}
}
